#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>


using namespace std;


int main(int argc, char* argv[]){

	typedef unsigned char MOTOR_TYPE;

	char* devicename = 0;
	MOTOR_TYPE leftWheelSpeed = 0;
	MOTOR_TYPE rightWheelSpeed = 0;
	if( argc > 3 )
	{
		devicename = argv[1];
		leftWheelSpeed = atoi(argv[2]);
		rightWheelSpeed = atoi(argv[3]);
		printf("Left: %d, Right: %d\n", leftWheelSpeed, rightWheelSpeed);
	}
	else
	{
		printf("Input requires /dev/ttyACM* for communicating to motor controller as first argument, followed by left and right wheel speeds in the range [-125, 125].");
		exit(-2);
	}

	// Open connection to motor controller
	int serialfd = open(devicename, O_WRONLY | O_NOCTTY);
	if(serialfd < 0)
	{
		char errmsg[] = "Unable to open serial connection to motor controller.";
		write(2, errmsg, strlen(errmsg));
		exit(-1);
	}
	
	// Configure serial settings
	struct termios portSettings;
	cfmakeraw(&portSettings);
	cfsetspeed(&portSettings, B9600);
	portSettings.c_cflag = CS8 | CRTSCTS | CLOCAL; //8N1
	tcsetattr(serialfd, TCSAFLUSH, &portSettings);

	// Set operating rate (must be at least 50 Hz for proper servo control)
	//while(1)
	{
		write(serialfd, &leftWheelSpeed, sizeof(MOTOR_TYPE));
		//write(serialfd, &rightWheelSpeed, sizeof(MOTOR_TYPE));

 		//usleep(16667);
	}

  return 0;
}

